package com.gitee.wsl.mathematics.complex.quaternion.ext

import com.gitee.wsl.mathematics.complex.quaternion.Quaternion
import com.gitee.wsl.unit.angle.AngleUnit
import com.gitee.wsl.unit.angle.ext.cos
import com.gitee.wsl.unit.angle.ext.sin
import com.gitee.wsl.unit.angle.ext.times

/** Compute the rotation between two vectors.
 *  param orig vector, needs to be normalized
 *  param dest vector, needs to be normalized
 *  @see gtx_quaternion */
/*
fun<T:Number> Quaternion<T, *>.rotation(orig: Vector3<T,*>, dest: Vector3<T,*>){

    val cosTheta = orig dot dest

    if (cosTheta >= 1 - epsilonF)
    // orig and dest point in the same direction
        return quatIdentity()

    if (cosTheta < -1 + epsilonF) {
        */
/*  special case when vectors in opposite directions :
            there is no "ideal" rotation axis
            So guess one; any will do as long as it's perpendicular to start
            This implementation favors a rotation around the Up axis (Y), since it's often what you want to do. *//*

        var rotationAxis = createVec3(0, 0, 1) cross orig
        if (rotationAxis.length2() < epsilonF) // bad luck, they were parallel, try again!
            rotationAxis = createVec3(1, 0, 0) cross orig

        rotationAxis = rotationAxis.normalize()
        return angleAxis(PIf, rotationAxis)
    }

    // Implementation from Stan Melax's Game Programming Gems 1 article
    val rotationAxis = orig cross dest

    val s = sqrt((1 + cosTheta) * 2)
    val invs = 1 / s

    return set(
        s * 0.5f,
        rotationAxis.x * invs,
        rotationAxis.y * invs,
        rotationAxis.z * invs)
}*/


/**
 * Calculate a quaternion rotate around X axis.
 * @param rad - The rotation angle in radians
 */
fun<T:Number> Quaternion<T, *>.rotationX(rad:AngleUnit) {
    val rad1 = rad * 0.5
    val s = rad1.sin()
    val c = rad1.cos()

    x = s+zero
    y = zero
    z = zero
    w = c+zero
}

/**
 * Calculate a quaternion rotate around Y axis.
 * @param rad - The rotation angle in radians
 */
fun<T:Number> Quaternion<T, *>.rotationY(rad:AngleUnit) {
    val rad1 = rad * 0.5
    val s = rad1.sin()
    val c = rad1.cos()

    x = zero
    y = s+zero
    z = zero
    w = c+zero
}

/**
 * Calculate a quaternion rotate around Y axis.
 * @param rad - The rotation angle in radians
 */
fun<T:Number> Quaternion<T, *>.rotationZ(rad:AngleUnit) {
    val rad1 = rad * 0.5
    val s = rad1.sin()
    val c = rad1.cos()

    x = zero
    y = zero
    z = s+zero
    w = c+zero
}